# -*- coding: utf-8 -*-
import rospy
from geometry_msgs.msg import PoseStamped
from tf.transformations import quaternion_from_euler
from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import MoveBaseActionResult
from std_msgs.msg import String, Float32MultiArray
import re

goal_points = {1: (4.89, -6.48, -0.3), 2: (3.90, -7.35, -1.86), 3: (2.50, -2.7, 2.9)}
grasp_messages = {1: "1", 2: "2", 3: "3", 4: "4", 5: "5", 6: "6"}


class CarRobotCommunicator:
    def __init__(self):
        rospy.init_node('multi_point_navigation')
        self.goal_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
        self.status_sub = rospy.Subscriber('/move_base/result', MoveBaseActionResult, self.goal_status_callback,
                                           queue_size=10)
        self.car_status_pub = rospy.Publisher("car_statue", Float32MultiArray, queue_size=10)
        self.task_sub = rospy.Subscriber("chemical_task", String, self.task_callback, queue_size=10)
        self.current_goal = None
        self.first_goal = None
        self.second_goal = None
        self.grasp_msg_id = None
        self.first_goal0 = None
        self.second_goal0 = None
        self.grasp_msg_id0 = None
        rospy.sleep(1)  # 等待节点初始化完成

    def task_callback(self, msg):
        # 清理并解析从"chemical_task"话题接收到的消息
        data = re.sub(r'\D', '', msg.data)  # 移除非数字字符
        if len(data) != 3:
            rospy.logerr("Received invalid message format: %s", msg.data)
            return

        self.first_goal0, self.second_goal0, self.grasp_msg_id0 = map(int, list(data))

        if self.first_goal0 not in goal_points or self.second_goal0 not in goal_points or not (1 <= self.grasp_msg_id0 <= 6):
            rospy.logerr("Invalid goal points or grasp message ID.")
            return

        self.first_goal = goal_points[self.first_goal0]
        self.second_goal = goal_points[self.second_goal0]
        self.grasp_msg_id = self.grasp_msg_id0
        self.current_goal = self.first_goal  # 初始化当前目标为第一个目标点
        self.send_initial_goal()

    def send_goal(self, goal_point):
        goal = PoseStamped()
        goal.header.frame_id = "map"

        goal.pose.position.x = goal_point[0]  # x
        goal.pose.position.y = goal_point[1]  # y

        quaternion = quaternion_from_euler(0, 0, goal_point[2])
        goal.pose.orientation.x = quaternion[0]
        goal.pose.orientation.y = quaternion[1]
        goal.pose.orientation.z = quaternion[2]
        goal.pose.orientation.w = quaternion[3]

        self.goal_pub.publish(goal)
        print("x=", goal_point[0], " y=", goal_point[1], " theta=", goal_point[2])  # 打印目标点信息

    def goal_status_callback(self, msg):
        if msg.status.status == 3:  # 当机器人到达目标点
            if self.current_goal == self.first_goal:
                # 到达第一个目标点
                self.notify_arm_grasp()
                self.await_arm_grasp_confirmation()
                self.current_goal = self.second_goal  # 更新当前目标为第二个目标点
                self.send_goal(self.current_goal)  # 发送第二个目标点
            elif self.current_goal == self.second_goal:
                # 到达第二个目标点
                self.notify_arm_place()
                self.await_arm_place_confirmation()
                self.reset_state_for_new_task()  # 完成后重置状态，准备接受下一个任务

    def notify_arm_grasp(self):
        for _ in range(20):  # 发送2s
            data_list = [self.first_goal0,self.second_goal0,self.grasp_msg_id]
            data = Float32MultiArray(data=data_list)
            # ros_message = "[{}, {}, {}]".format(self.first_goal0,self.second_goal0,self.grasp_msg_id)
            # send_data.data = [self.first_goal0,self.second_goal0,self.grasp_msg_id]
            print(data)
            self.car_status_pub.publish(data)
            rospy.sleep(0.1)

    def await_arm_grasp_confirmation(self):
        msg = rospy.wait_for_message("arm_statue", String)
        print("消息：", msg.data)

    def notify_arm_place(self):
        for _ in range(20):  # 发送2s
            send_data = Float32MultiArray(data=[1])
            # send_data.data = 1
            self.car_status_pub.publish(send_data)
            print(send_data.data)
            rospy.sleep(0.1)

    def await_arm_place_confirmation(self):
        msg = rospy.wait_for_message("arm_statue", String)
        print("消息：", msg.data)

    def reset_state_for_new_task(self):
        """重置状态以准备接收下一个任务"""
        self.first_goal = None
        self.second_goal = None
        self.grasp_msg_id = None
        self.current_goal = None
        print("Ready for new task.")

    def send_initial_goal(self):
        self.send_goal(self.first_goal)
        print("sent initial goal*********")


if __name__ == '__main__':
    communicator = CarRobotCommunicator()
    rospy.spin()